/**
* This file is part of DPPTAM.
*
* Copyright (C) 2015 Alejo Concha Belenguer <alejocb at unizar dot es> (University of Zaragoza)
* and Javier Civera Sancho   <jcivera at unizar dot es> (University of Zaragoza)
*
* DPPTAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* DPPTAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with DPPTAM. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef __SEMIDENSEMAPPING_H
#define __SEMIDENSEMAPPING_H

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <stdio.h>
#include <stack>
#include <ctime>

//chadir
#include <unistd.h>
// reading a text file
#include <iostream>
#include <fstream>
#include <string>
//directorio
#include <dirent.h>
//#include <ros/ros.h>

//#include "superpixel.h"
#include <dpptam/DenseMapping.h>
#include <dpptam/SemiDenseTracking.h>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
//#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include "opencv2/imgproc/imgproc.hpp"


//#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>


// TIC - TOC
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <math.h>

/*#define U_SEGS(a)\
         gettimeofday(&tv,0);\
         a = tv.tv_sec + tv.tv_usec/1000000.0*/
class SemiDenseTracking;

class SemiDenseMapping : public Imagenes {
public:
    SemiDenseMapping(const cv::FileStorage &fs2, DenseMapping *_dense_mapper, MapShared *_Map);

    vector<cv::Mat> get_points_new_map() { return _points_new_map; }

    void init_points_new_map(int);

    void set_map_points_print(cv::Mat);

    void set_points_new_map(vector<cv::Mat>);

    void get_ph_errors_sd_exh(Imagenes &images, int reference_image, int image_to_be_added,
                              int discretization, int window_size, cv::Mat &epipolar_gradients, float &ratio_parallax,
                              bool optimize_previous_frame);

    void convergence_test(cv::Mat &be_outlier, cv::Mat &be_outlier_print, cv::Mat &deviation_inv_depth,
                          cv::Mat &final_variances, float inv_depth_disparity_th, float idepth_disp_print_th);

    void set_tracker(SemiDenseTracking *_semidense_tracker);

    void run(Imagenes *images, Imagenes *pre_kf_images, ros::Publisher *pub_cloud);

    void run_core(Imagenes *pimages, Imagenes *pimages_previous_keyframe, ros::Publisher *pub_cloud);

    cv::Mat get_map_points_print();

    float _mean_depth_value;
    float _convergence;
    float _convergence_total;

    int _previous_images;
    int _do_initialization;
    int _do_init_track;
    int _do_optimization;
    int _do_var_mapping;

//    pcl::PointCloud<pcl::PointXYZRGB> map_pcl;

    int _num_cam_map;
    int _init_keyframes;

    int _last_frame_mapped;
    int _last_frame_tracked;
    double _translational_ratio_th_min;
    double _translational_ratio_th_min_aux;
    double _translational_ratio_th;
    double _translational_ratio;

    float _overlap_tracking;
    int _images_size;

    int num_cam_map_th;
    int num_cameras_mapping_th_aux;
    int num_keyframes;

    int do_init_semi;

    PhotometricTerm _term;
    PhotometricTerm _gradient_magnitude;
    PhotometricTerm _gx_ex;
    PhotometricTerm _gy_ey;

    cv::Mat _g_expanded;
    cv::Mat _gx;
    cv::Mat _gy;
    cv::Mat _max_idepth_init_seed;
    cv::Mat _min_idepth_init_seed;

    cv::Mat _points_by_depth;

    PhotometricTerm _point_limits_for_sd;

    cv::Mat _points_ref_im_sd;
    cv::Mat _t_r_ref;
    cv::Mat _init_idepth_sd;
    vector<cv::Mat> _init_idepth_xun;
    vector<cv::Mat> _init_idepth_xlpara;
    vector<cv::Mat> _previous_or_next_frame;
    cv::Mat _image_points_by_focal_sd;

    cv::Mat _pixel_taken;

    int _kinect_initialization;
    float _limit_grad;

    float _mean_value;
    float _depth_step;
    cv::Mat _inv_depths;

    int _frames_previous_keyframe_processed;
    int _pre_kf_used;

    cv::Mat _map_points;
    cv::Mat _local_map_points;

    cv::Mat _points_last_keyframe;

    cv::Mat _points_3d_tracked;
    cv::Mat _weight_tracked_points;
    cv::Mat _image_coordinates_tracked_points;
    cv::Mat _points3D_tracked_to_map;
    cv::Mat _depth_map_points_tracked;
    cv::Mat _variance_points_tracked;

    cv::Mat _depth_map;

    cv::Mat _b;
    cv::Mat _bb;
    float _overlap_tracking_th;

    vector<cv::Mat> _points3D_toprint;

    DenseMapping *dense_mapper;
    SemiDenseTracking *semidense_tracker;
    MapShared *Map;

private:
    vector<cv::Mat> _points_new_map;

    cv::Mat _map_points_print;

protected:
    static boost::mutex mutex_p;


};

class SemiDenseTracking;  // It is defined also here due to cross reference (#include) issues

///semidesen mapper thread
void ThreadSemiDenseMapper(Imagenes *images, Imagenes *images_previous_keyframe, SemiDenseMapping *semidense_mapper,
                           SemiDenseTracking *semidense_tracker, DenseMapping *dense_mapper, MapShared *Map,
                           ros::Publisher *pub_cloud);

///semidense mapper function
void
semidense_mapping(DenseMapping *dense_mapper, SemiDenseMapping *semidense_mapper, SemiDenseTracking *semidense_tracker,
                  MapShared *Map, Imagenes *pimages, Imagenes *pimages_previous_keyframe, ros::Publisher *pub_cloud);

void copy_first_and_last_images(Imagenes &images, Imagenes &images_map);

///Calculate the photometric reprojection error of the high gradient points
void get_photometric_errors_matrix_sd_exhaustive(Imagenes &images, cv::Mat &inv_depths, PhotometricTerm &X,
                                                 PhotometricTerm &X_gradient_Magnitude, PhotometricTerm &X_gx_ex,
                                                 PhotometricTerm &X_gy_ey, int reference_image,
                                                 cv::Mat &initial_inv_depth, int image_to_be_added,
                                                 PhotometricTerm &points_i_todos, cv::Mat &points_ref_im_sd,
                                                 int discretization, int window_size, cv::Mat &epipolar_gradients,
                                                 vector<cv::Mat> &initial_inv_depth_inEveryCamera_uncertainty,
                                                 vector<cv::Mat> &initial_inv_depth_inEveryCamera_largeParallax,
                                                 cv::Mat &points_by_depth, cv::Mat &t_r_ref, float &ratio_parallax,
                                                 bool optimize_previous_frame, vector<cv::Mat> &previous_or_next_frame,
                                                 cv::Mat &GX, cv::Mat &GY, int &num_cameras_mapping,
                                                 cv::Mat &max_inv_depth_initial_seed,
                                                 cv::Mat &min_inv_depth_initial_seed);

///Filtering points of the semidense map that are not consistent temporally
void convergence_test(SemiDenseMapping *semidense_mapper, cv::Mat &be_outlier,
                      cv::Mat &be_outlier_print, cv::Mat &deviation_inv_depth, cv::Mat &final_variances,
                      float inv_depth_disparity_th, float inv_depth_disparity_print_th);

void find_closest_maps(SemiDenseMapping *semidense_mapper, MapShared *Map, SemiDenseTracking *semidense_tracker);


void join_last_images(Imagenes *images, Imagenes *images_previous_keyframe, DenseMapping *dense_mapper,
                      SemiDenseMapping *semidense_mapper);

void copy_imagenes_dense(Imagenes &images, Imagenes &images_map);

template<typename T>
void filter_imagenes(T &images_dense, int num_keyframes);

void copy_imagenes(const Imagenes &images, Imagenes &images_map);

#endif
